/*******************************************************************************
*   Copyright (C) Harbin Institute of Technology, Shenzhen.
*				  All Rights Reserved.
*   Department: Decoration
********************************************************************************
* File Name   : ros.c
* Author      : Dayuan
* Version     : v0.01
* Date        : 2019/9/6
* Description : Decoration Rototics Manipulator task.
*******************************************************************************/
/*-History----------------------------------------------------------------------
* Version   Date      Name        Changes and comments
* v0.01     2019/9/6  Dayuan    initial version
*=============================================================================*/
/* System Includes -----------------------------------------------------------*/

/* Public Includes -----------------------------------------------------------*/
#include "usart.h"
#include "ros.h"
#include "ros_if.h"

/* Private Includes ----------------------------------------------------------*/
static MAIN_MODE main_mode = GB_STOP;
static SUB_MODE sub_mode = GB_POSITION;

static JOINT_INFO joint_cb; // define receive buf

static uint8_t impadence_inital;
/* Global variables ----------------------------------------------------------*/



/* Private variables ---------------------------------------------------------*/
static ROS_TX ros_tx;	// define transfer msg
static ROS_INFO ros_rx;
static uint8_t ros_cmd_rx_flag = 0;	// define recive msg
static uint8_t ros_data_rx_flag = 0;	// define recive msg
/* Global functions ----------------------------------------------------------*/

void ros_initial(void)
{
	main_mode = GB_STOP;
  sub_mode = GB_POSITION;
	impadence_inital = 0;
	ros_cmd_rx_flag = 0;
	ros_data_rx_flag = 0;
	HAL_UART_Receive_DMA(&huart3,ros_rx.buff,ROS_RX_BUFF_LEN);
}

void ros_send_data(JOINT_INFO *joint)
{
	ros_tx.head = 0xA55A;
	ros_tx.angle = float2int(joint->inc[CURRENT], COM_TYPE_ANGLE);
	ros_tx.force = float2int(joint->f_act, COM_TYPE_FORCE);
	ros_tx.status = (joint->error_code <<4 & 0xf0)| joint->status;
	
	ros_tx.buff[0] = ros_tx.head>>8 & 0x00ff;
	ros_tx.buff[1] = ros_tx.head & 0x00ff;
	ros_tx.buff[2] = ros_tx.angle>>8 & 0x00ff;
	ros_tx.buff[3] = ros_tx.angle & 0x00ff;
	ros_tx.buff[4] = ros_tx.force>>8 & 0x00ff;
	ros_tx.buff[5] = ros_tx.force & 0x00ff;
	ros_tx.buff[6] = ros_tx.status;
	
	ros_tx.crc = enc_tawa_crc_cal(ros_tx.buff, ROS_TX_BUFF_LEN - 1);
	ros_tx.buff[7] = ros_tx.crc;
	
	HAL_UART_Transmit_DMA(&huart3,ros_tx.buff,ROS_TX_BUFF_LEN);
}

void ros_rx_callback(void)
{
	HAL_UART_Receive_DMA(&huart3,ros_rx.buff,ROS_RX_BUFF_LEN);
	ros_rx.head = ros_rx.buff[0] <<8 | ros_rx.buff[1];
	if(ros_rx.head == 0xA55A)
	{
		ros_rx.crc = ros_rx.buff[5];
		if(ros_rx.crc == enc_tawa_crc_cal(ros_rx.buff,ROS_RX_BUFF_LEN - 1))
		{
			ros_rx.cmd = ros_rx.buff[2];
			ros_rx.data = ros_rx.buff[3] <<8 | ros_rx.buff[4];
		
			switch (ros_rx.cmd)
			{
				case GB_SET:
				{
					main_mode = (MAIN_MODE)((ros_rx.data >> 8) & 0x00ff);
					sub_mode = (SUB_MODE)(ros_rx.data & 0x00ff);
					if(main_mode == GB_RESTART)
					{
						reset_ros();
					}
					ros_cmd_rx_flag = 1;
					break;
				}
				case GB_POSE:
				{
					joint_cb.pose = int2float(ros_rx.data, COM_TYPE_ANGLE);
					ros_data_rx_flag = 1;
					break;
				}
				case GB_ANGLED:
				{
					joint_cb.angle_des = int2float(ros_rx.data, COM_TYPE_ANGLE);
					ros_data_rx_flag = 1;
					break;
				}
				case GB_KD_DESIRED:
				{
					joint_cb.k_of_im = int2float(ros_rx.data, COM_TYPE_K);
					joint_cb.d_of_im = int2float(ros_rx.data, COM_TYPE_D);
					ros_data_rx_flag = 1;
					impadence_inital = 1; 
					break;
				}
				default:break;
			}
		}
		else
			ros_rx.crc_error++;
	}
}


int parse_ros_info(SYS_CMD *sys, JOINT_INFO *joint)
{
	uint8_t parse_success = 0;
	
	if(ros_cmd_rx_flag !=0)
	{
		ros_cmd_rx_flag = 0;
		sys->main_mode = main_mode;
		if(sub_mode == GB_IMPADENCE && impadence_inital == 0x01)
			sys->sub_mode = sub_mode;
		else if(sub_mode != GB_IMPADENCE)
			sys->sub_mode = sub_mode;
		parse_success |= 1 <<0;
	}
	
	if (ros_data_rx_flag != 0)
	{
		ros_data_rx_flag = 0;
		joint->pose = joint_cb.pose;
		joint->angle_des = joint_cb.angle_des;
		if(sub_mode == GB_IMPADENCE)
		{
			joint->k_of_im = joint_cb.k_of_im;
			joint->d_of_im = joint_cb.d_of_im;
		}
		parse_success |= 1 << 1;
	}
	return parse_success;
}



/* Private functions ---------------------------------------------------------*/
int reset_ros(void)
{
	main_mode = GB_STOP;
  sub_mode = GB_POSITION;
	impadence_inital = 0;
	ros_cmd_rx_flag = 0;
	ros_data_rx_flag = 0;
	return 0;
}

int16_t float2int(float data, COM_DATA_TYPE type)
{
	float tempf;
	int16_t tempi;
	switch(type)
	{
		case(COM_TYPE_ANGLE):
		{
			if(data < COM_ANGLE_MAX && data > -COM_ANGLE_MAX)
				tempf = data;
			else
				tempf = COM_ANGLE_MAX - data;
			tempi = (tempf / COM_ANGLE_MAX) * COM_RES_MAX;
			break;
		};
		case(COM_TYPE_FORCE):
		{
			tempf = data;
			tempi = (tempf / COM_FORCE_MAX) * COM_RES_MAX;
			break;
		}
		default:;		
	}
	return tempi;
}

float int2float(int16_t data, COM_DATA_TYPE type)
{
	float tempf;
	int16_t tempi = data;
	switch(type)
	{
		case(COM_TYPE_ANGLE):
		{
			tempf = ((float)tempi / COM_RES_MAX) * COM_ANGLE_MAX;
			break;
		};
		case(COM_TYPE_FORCE):
		{
			tempf = ((float)tempi / COM_RES_MAX) * COM_FORCE_MAX;
			break;
		}
		case(COM_TYPE_K):
		{
			tempf = ((float)((tempi >>8) & 0x00ff) / COM_RES_MAX_2) * COM_K_MAX;
			break;
		}
		case(COM_TYPE_D):
		{
			tempf = ((float)(tempi & 0x00ff) / COM_RES_MAX_2) * COM_D_MAX;
			break;
		}
		default:;		
	}
	return tempf;
}
uint8_t enc_crc8_update(uint8_t crc, uint8_t data)
{
	uint8_t i;
	crc = crc ^ data;
	for (i = 0; i < 8; i++)
	{
		if(crc & 0x01)
		{
			crc = (crc >> 1) ^ 0x80;
		}
		else
		{
			crc >>= 1;
		}
	}
	return crc;
}

uint8_t enc_tawa_crc_cal(uint8_t *buf, uint8_t size)
{
	uint8_t temp_crc = 0x00;
	for(uint8_t i=0; i<size; i++)
	{
		temp_crc = enc_crc8_update(temp_crc, buf[i]);
	}
	return temp_crc;
}
/****************************** END OF FILE ***********************************/
